#include "MyISP.h"
#include <stdio.h> 
#include <math.h>
#include <string.h>

void EE_Sobel(ImageYUV& yuv, float gain)
{
    // Sobel
    float sobel_x[3][3] = {
        {-1, 0, 1},
        {-2, 0, 2},
        {-1, 0, 1}
    };
    float sobel_y[3][3] = {
        {-1, -2, -1},
        {0, 0, 0},
        {1, 2, 1}
    };

    ImageYUV* yuv_pad = new ImageYUV(yuv);
    yuv_pad->padding(1);

    for (int y = 0; y < yuv.getHeight(); y++)
    {
        for (int x = 0; x < yuv.getWidth(); x++)
        {
            float Y = yuv.at(y, x).Y;
            float grad_x = 0, grad_y = 0;

            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    grad_x += sobel_x[i][j] * yuv_pad->at(y + i, x + j).Y;
                    grad_y += sobel_y[i][j] * yuv_pad->at(y + i, x + j).Y;
                }
            }

            float gradient = sqrt(grad_x * grad_x + grad_y * grad_y);

            Y = Y + gain * gradient;
            if (Y < 0) Y = 0;
            if (Y > 255) Y = 255;

            yuv.at(y, x).Y = Y;
        }
    }

    delete yuv_pad;
}